/*
 * regler.cpp
 *
 *      Author: Jonas
 *      Letztes Update: 03.11.13 Christian | Code aufräumen
 *
 */

#include "regler.h"

#ifdef FLOAT_REGLER

int8_t motor_werte[4];

uint8_t anteil_p = 0;
uint8_t anteil_i = 0;
uint8_t anteil_d = 0;

uint8_t p_z = 0;
uint8_t i_z = 0;
uint8_t d_z = 0;

float e_x = 0;
float e_y = 0;
float e_z = 0;

float e_x_sum = 0;
float e_y_sum = 0;
float e_z_sum = 0;

float e_x_old = 0;
float e_y_old = 0;
float e_z_old = 0;

float regler_x_temp = 0;
float regler_y_temp = 0;
float regler_z_temp = 0;

int8_t reglerWert_x = 0;
int8_t reglerWert_y = 0;

#define dt (ABTASTZEIT_IN_MS / 1000.0 )

void calcMotorValues(uint8_t throttle, float winkel_x, float winkel_y,
		int16_t v_Z) {
	//Throttle limitieren
	if (throttle > THROTTLE_MAX) {
		throttle = THROTTLE_MAX;
	}

	if (throttle > REGLER_MIN) {
		e_x = winkel_x / 3.0 - x.angle;
		e_y = winkel_y / 3.0 - y.angle;

		e_x_sum += e_x;
		e_y_sum += e_y;

		if (e_x_sum > 30) {

			e_x_sum = 30.0;
		} else if (e_x_sum < -30) {

			e_x_sum = -30.0;
		}

		if (e_y_sum > 30) {

			e_y_sum = 30.0;
		} else if (e_y_sum < -30) {

			e_y_sum = -30.0;
		}

		//Neue Werte mittels PID-Regler ausrechnen
		regler_x_temp =
				((float) anteil_p)
						* e_x+ ( (float) anteil_i ) * dt * e_x_sum + ( (float) anteil_d ) * (e_x - e_x_old) / dt;
		regler_y_temp =
				((float) anteil_p)
						* e_y+ ( (float) anteil_i ) * dt * e_y_sum + ( (float) anteil_d ) * (e_y - e_y_old) / dt;

		reglerWert_x = (int8_t) (regler_x_temp / 100);
		reglerWert_y = (int8_t) (regler_y_temp / 100);

		//Überprüfen, ob Reglerwerte Limit überschreiten, ggf. verkleinern

		// X-LIMITS

		if (reglerWert_x > REGLER_LIMIT) {

			reglerWert_x = REGLER_LIMIT;
		} else if (reglerWert_x < 0) {

			if (-1 * reglerWert_x > REGLER_LIMIT) {

				reglerWert_x = -1 * REGLER_LIMIT;
			}
		}

		if ((throttle + reglerWert_x) < REGLER_MIN) {

			reglerWert_x = REGLER_MIN - throttle;
		} else if ((throttle - reglerWert_x) < REGLER_MIN) {

			reglerWert_x = throttle - REGLER_MIN;
		}

		// Y-LIMITS

		if (reglerWert_y > REGLER_LIMIT) {

			reglerWert_y = REGLER_LIMIT;
		} else if (reglerWert_y < 0) {

			if (-1 * reglerWert_y > REGLER_LIMIT) {

				reglerWert_y = -1 * REGLER_LIMIT;
			}
		}

		if ((throttle + reglerWert_y) < REGLER_MIN) {

			reglerWert_y = REGLER_MIN - throttle;
		} else if ((throttle - reglerWert_y) < REGLER_MIN) {

			reglerWert_y = throttle - REGLER_MIN;
		}

	} else {
		reglerWert_x = 0;
		reglerWert_y = 0;
	}

	//Reglerwerte auf throttle addieren und als neue Motorwerte ablegen
	motor_werte[0] = throttle - reglerWert_x;
	motor_werte[1] = throttle + reglerWert_y;
	motor_werte[2] = throttle + reglerWert_x;
	motor_werte[3] = throttle - reglerWert_y;

	//Winkel zwischenspeichern für D-Anteil
	e_x_old = e_x;
	e_y_old = e_y;

	if (throttle > REGLER_MIN) {
		calc_Z(v_Z);
	}
}

void calc_Z(int16_t v) {

	if (abs((int16_t) (z.rate * 10) > VZ_MIN)) {

		e_z = v - (z.rate * 10);
		e_z_sum += e_z;

		// Begrenzung fuer Integrator Summe, wegen ueberlauf.

		if (e_z_sum > 30) {

			e_z_sum = 30;
		} else if (e_z_sum < -30) {

			e_z_sum = -30;
		}

		regler_z_temp = (float) p_z
				* e_z+ (float) i_z * dt * e_z_sum + d_z * (e_z - e_z_old) / dt;
		regler_z_temp /= 100.0;
		e_z_old = e_z;
		// LIMITS

		if (regler_z_temp > Z_LIMIT) {
			regler_z_temp = Z_LIMIT;
		} else if (regler_z_temp < 0) {
			if (-1 * regler_z_temp > Z_LIMIT) {
				regler_z_temp = -1 * Z_LIMIT;
			}
		}

		motor_werte[0] -= (int8_t) regler_z_temp;
		motor_werte[1] += (int8_t) regler_z_temp;
		motor_werte[2] -= (int8_t) regler_z_temp;
		motor_werte[3] += (int8_t) regler_z_temp;

	}
}

#else

int8_t motor_werte[4];

int8_t reglerWert_x = 0;
int8_t reglerWert_y = 0;
int16_t abweichung_x_summe = 0;
int16_t abweichung_y_summe = 0;
int16_t differenz_x_alt = 0;
int16_t differenz_y_alt = 0;
int16_t differenz_x = 0;
int16_t differenz_y = 0;

int32_t regler_x_temp = 0;
int32_t regler_y_temp = 0;

uint8_t anteil_p = 0;
uint8_t anteil_i = 0;
uint8_t anteil_d = 0;

uint8_t p_z = 0;
uint8_t i_z = 0;
uint8_t d_z = 0;

int16_t e_z_sum = 0;			// error sum
int32_t z_temp = 0;
int16_t e_z = 0;// error_z
int16_t e_z_old = 0;
/*
 * Hier werden die Werte für die Motoren aus dem aktuellen Winkel und dem Wunschwinkel kalkuliert
 */
void calcMotorValues(uint8_t throttle, int16_t winkel_x, int16_t winkel_y, int16_t v_Z ) {
	//Throttle limitieren
	if(throttle > THROTTLE_MAX) {

		throttle = THROTTLE_MAX;
	}

	if ( throttle > REGLER_MIN ) {

		//Winkel Integrieren
		differenz_x = winkel_x - angle_x;
		differenz_y = winkel_y - angle_y;

//		differenz_x /=10;
//		differenz_y /=10;

		abweichung_x_summe += differenz_x;
		abweichung_y_summe += differenz_y;
		differenz_x /=10;
		differenz_y /=10;
		// Begrenzung fuer Integrator Summe, wegen ueberlauf.

		if ( abweichung_x_summe > 30000 ) {

			abweichung_x_summe = 30000;
		} else if ( abweichung_x_summe < -30000 ) {

			abweichung_x_summe = -30000;
		}

		if ( abweichung_y_summe > 30000 ) {

			abweichung_y_summe = 30000;
		} else if ( abweichung_y_summe < -30000 ) {

			abweichung_y_summe = -30000;
		}

		//Neue Werte mittels PID-Regler ausrechnen
		regler_x_temp = anteil_p * differenz_x + anteil_i * ABTASTZEIT_IN_MS * abweichung_x_summe / 100 + anteil_d * (differenz_x - differenz_x_alt) * 10 / ABTASTZEIT_IN_MS;

		regler_y_temp = anteil_p * differenz_y + anteil_i * ABTASTZEIT_IN_MS * abweichung_y_summe / 100 + anteil_d * (differenz_y - differenz_y_alt) * 10 / ABTASTZEIT_IN_MS;

		reglerWert_x = (int8_t) ( regler_x_temp / 1000);
		reglerWert_y = (int8_t) (regler_y_temp / 1000);

		//Überprüfen, ob Reglerwerte Limit überschreiten, ggf. verkleinern

		// X-LIMITS

		if(reglerWert_x > REGLER_LIMIT) {

			reglerWert_x = REGLER_LIMIT;
		} else if ( reglerWert_x < 0 ) {

			if ( -1 * reglerWert_x > REGLER_LIMIT) {

				reglerWert_x = -1 * REGLER_LIMIT;
			}
		}

		if (( throttle + reglerWert_x ) < REGLER_MIN) {

			reglerWert_x = REGLER_MIN - throttle;
		} else if (( throttle - reglerWert_x ) < REGLER_MIN) {

			reglerWert_x = throttle - REGLER_MIN;
		}

		// Y-LIMITS

		if(reglerWert_y > REGLER_LIMIT) {

			reglerWert_y = REGLER_LIMIT;
		} else if ( reglerWert_y < 0 ) {

			if ( -1 * reglerWert_y > REGLER_LIMIT) {

				reglerWert_y = -1 * REGLER_LIMIT;
			}
		}

		if (( throttle + reglerWert_y ) < REGLER_MIN) {

			reglerWert_y = REGLER_MIN - throttle;
		} else if (( throttle - reglerWert_y ) < REGLER_MIN) {

			reglerWert_y = throttle - REGLER_MIN;
		}

	} else {
		reglerWert_x = 0;
		reglerWert_y = 0;
	}

	//Reglerwerte auf throttle addieren und als neue Motorwerte ablegen
	motor_werte[0] = throttle - reglerWert_x;
	motor_werte[1] = throttle + reglerWert_y;
	motor_werte[2] = throttle + reglerWert_x;
	motor_werte[3] = throttle - reglerWert_y;

	//Winkel zwischenspeichern für D-Anteil
	differenz_x_alt = differenz_x;
	differenz_y_alt = differenz_y;

	if ( throttle > REGLER_MIN ) {
		calc_Z( v_Z);
	}
}

void calc_Z(int16_t v) {

	if ( abs(gyro_rate_z) > VZ_MIN ) {

		e_z = v - gyro_rate_z;
		e_z_sum += e_z;

		// Begrenzung fuer Integrator Summe, wegen ueberlauf.

		if ( e_z_sum > 30000 ) {

			e_z_sum = 30000;
		} else if ( e_z_sum < -30000 ) {

			e_z_sum = -30000;
		}

		z_temp = p_z * e_z + i_z * ABTASTZEIT_IN_MS * e_z_sum / 200 + d_z * (e_z - e_z_old) * 10 / ABTASTZEIT_IN_MS;

		z_temp = z_temp / 10;

		e_z_old = e_z;
		// LIMITS

		if ( z_temp > Z_LIMIT ) {
			z_temp = Z_LIMIT;
		} else if ( z_temp < 0 ) {
			if ( -1 * z_temp > Z_LIMIT ) {
				z_temp = -1 * Z_LIMIT;
			}
		}

		motor_werte[0] -= z_temp;
		motor_werte[1] += z_temp;
		motor_werte[2] -= z_temp;
		motor_werte[3] += z_temp;

	}
}

#endif

void regler_init() {

	//Auslesen der Reglerwerte
	anteil_p = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_XY);
	anteil_i = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_XY);
	anteil_d = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_XY);

	p_z = eeprom_read_byte((uint8_t *) EEPROM_ADR_P_Z);
	i_z = eeprom_read_byte((uint8_t *) EEPROM_ADR_I_Z);
	d_z = eeprom_read_byte((uint8_t *) EEPROM_ADR_D_Z);
}

void reglerwerte_update() {
	system_pause();
	// Alte Reglerwerte Senden

	send_buffer[0] = anteil_p;
	send_buffer[1] = anteil_i;
	send_buffer[2] = anteil_d;

	send_buffer[3] = p_z;
	send_buffer[4] = i_z;
	send_buffer[5] = d_z;

	UART_send(6);

	// Neue Reglerwerte Empfangen
	// TODO : interruptgesteuerten Empfang benutzen

	while (!(UCSRA & (1 << RXC)))
		;
	anteil_p = UDR;

	while (!(UCSRA & (1 << RXC)))
		;
	anteil_i = UDR;

	while (!(UCSRA & (1 << RXC)))
		;
	anteil_d = UDR;

	while (!(UCSRA & (1 << RXC)))
		;
	p_z = UDR;

	while (!(UCSRA & (1 << RXC)))
		;
	i_z = UDR;

	while (!(UCSRA & (1 << RXC)))
		;
	d_z = UDR;

	// Daten in EEPROM schreiben

	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_XY, anteil_p);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_XY, anteil_i);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_XY, anteil_d);

	eeprom_update_byte((uint8_t *) EEPROM_ADR_P_Z, p_z);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_I_Z, i_z);
	eeprom_update_byte((uint8_t *) EEPROM_ADR_D_Z, d_z);

	system_reset();
}
